#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
#include "cam_cap_node_0/sensor_diagnosis.h"

//这行用于声明节点使用的消息文件所在的命名空间。集成编译时删掉下面这行 
using namespace cam_cap_node_0;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "cam_cap_node_0");
  ros::NodeHandle nh;

  //使用cv::VideoCapture创建容器，获取摄像头图像
  const int CAMERA_INDEX = 0;
  const float FRPS = 25;
  cv::VideoCapture cap(CAMERA_INDEX);

  //创建诊断信号发布器对象
  ros::Publisher pub_diagnosis = nh.advertise<sensor_diagnosis>("cam_0_diag", 100);
  sensor_diagnosis cam_diag;

  //如果摄像头数据无法读取，则关闭这个节点
  if (not cap.isOpened())
  {
    ROS_ERROR_STREAM("Failed to open camera with index " << CAMERA_INDEX << "!");
    cam_diag.is_work = false;
    cam_diag.header.stamp = ros::Time::now(); 
    pub_diagnosis.publish(cam_diag);
    nh.shutdown();
  }

  //创建图像转换对象、图像发布器对象
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub_image = it.advertise("cam_cap_0", 1); //发布到cam_cap话题。图像发布队列长度只能是1

  //创建一个智能指针（shared_ptr），指向一个cv_bridge::CvImage对象
  cv_bridge::CvImagePtr frame = boost::make_shared<cv_bridge::CvImage>();
  //定义图像编码为BGR8（CV_8UC3）
  frame->encoding = sensor_msgs::image_encodings::BGR8;

  ros::Rate loop_rate(FRPS);

  while (ros::ok())
  {
    //cap对象输出cv::Mat到frame对象的image成员
    cap >> frame->image;
    cam_diag.header.stamp = ros::Time::now(); 

    //如果图像不为空，则发布
    if (frame->image.empty())
    {
      ROS_ERROR_STREAM("Failed to capture frame!");
      cam_diag.is_work = false;
    }
    else
    {
      frame->header.stamp = ros::Time::now();
      cam_diag.is_work = true;
      pub_image.publish(frame->toImageMsg());
    }
    pub_diagnosis.publish(cam_diag);
    loop_rate.sleep();
  }

  cap.release();
  return EXIT_SUCCESS;
}
